#include "rot_ap.h"
#include "rot_tacho.h"
#include "rot_servo.h"

volatile uint16_t ctl_pos;
volatile int soma;

int pwm_to_rpm(unsigned int pwm_val) {

  int rpm;

  rpm = (int) ((float) (pwm_val-SERVO_MIN))*((float) (ROT_MAX-ROT_MIN))/((float) (SERVO_MAX-SERVO_MIN));
  rpm += ROT_MIN;

  return rpm; 

}

int drpm_to_dpwm(int drpm_val) {

  int pwm;

  pwm = (int) ((float) drpm_val * ((float) (SERVO_MAX-SERVO_MIN))/((float) (ROT_MAX-ROT_MIN)));

  return pwm; 

}

void ap_init(void) {

  ctl_pos = SERVO_MIN;
  soma = 0;

}

void run_ap(void) {

  int rpm_mes, rpm_des;
  int ctl_int, ctl_p;
  int err;

  rpm_mes = tacho_get_rpm();
  rpm_des = pwm_to_rpm(ppm_vals[MOTOR_CH]);
  
  err = rpm_des-rpm_mes;
  soma += err*5/INV_DELTA;

  soma = saturate(soma,
		  (ROT_MIN-ROT_MAX)*5,
		  (ROT_MAX-ROT_MIN)*5);
  
  ctl_int = drpm_to_dpwm(soma*CTL_KI_NUM/CTL_KI_DEN);
  ctl_p = drpm_to_dpwm(err*CTL_KP_NUM/CTL_KP_DEN);
  ctl_pos = SERVO_MIN + ctl_int + ctl_p;
  ctl_pos = saturate(ctl_pos,SERVO_MIN,SERVO_MAX);

}
